#ifndef R3D_STEREO_DEPTH_H
#define R3D_STEREO_DEPTH_H

#include "stereo_utils.h"
#include "camera.h"

#include <opencv2/opencv.hpp>
#include <opencv2/photo.hpp>

/**************** CAMERA SETTINGS ****************/
// main camera distortion type
#define R3D_IMAGE_DEPTH_ALG_NORMAL 1
#define R3D_IMAGE_DEPTH_ALG_FAST 2
#define R3D_IMAGE_DEPTH_ALG_BEST 3

// camera orientation
#define R3D_IMAGE_ROTATE_COUNTER_CLOCKWISE_90 1
#define R3D_IMAGE_ROTATE_CLOCKWISE_90 2
#define R3D_IMAGE_ROTATE_180 3
#define R3D_IMAGE_ROTATE_NONE 4

/*************************************************/

using namespace cv;

namespace r3d_depth_image {
	class R3DStereoDepth;

	struct StereoIntrinsics {
		cv::Mat cam_mat_sub;
		cv::Mat dist_coef_sub;
		cv::Mat cam_mat_main;
		cv::Mat dist_coef_main;

		void copy(StereoIntrinsics i_) {
			i_.cam_mat_sub.copyTo(this->cam_mat_sub);
			i_.dist_coef_sub.copyTo(this->dist_coef_sub);
			i_.cam_mat_main.copyTo(this->cam_mat_main);
			i_.dist_coef_main.copyTo(this->dist_coef_main);
		};
	};

	struct StereoExtrinsics {
		cv::Mat rot;
		cv::Mat trans;
		
		void copy(StereoExtrinsics e_) {
			e_.rot.copyTo(this->rot);
			e_.trans.copyTo(this->trans);
		};
	};

	struct ParamStereo {
		StereoIntrinsics intrinsics;
		StereoExtrinsics extrinsics;
		cv::Size full_size;
		cv::Size crop_size;
		cv::Size cali_size;
		cv::Size depth_size;
		int orientation;
		int alg_type;
		int num_thread;
		int max_disp;
		int win_size;
		int num_feat;
		int max_iter;
		float res_level;
		int num_worst;
		float min_disp;
		float delta_disp_per_degree;
		bool dump_enabled;
		cv::String dump_path;

		void copy(const ParamStereo & param_) {
			this->intrinsics.copy(param_.intrinsics);
			this->extrinsics.copy(param_.extrinsics);
			this->full_size = param_.full_size;
			this->crop_size = param_.crop_size;
			this->cali_size = param_.cali_size;
			this->depth_size = param_.depth_size;
			this->orientation = param_.orientation;
			this->alg_type = param_.alg_type;
			this->num_thread = param_.num_thread;
			this->max_disp = param_.max_disp;
			this->win_size = param_.win_size;
			this->num_feat = param_.num_feat;
			this->max_iter = param_.max_iter;
			this->res_level = param_.res_level;
			this->num_worst = param_.num_worst;
			this->min_disp = param_.min_disp;
			this->delta_disp_per_degree = param_.delta_disp_per_degree;
			this->dump_enabled = param_.dump_enabled;
			this->dump_path = param_.dump_path;
		}
	};


	class R3DStereoDepth
	{
	public:
		R3DStereoDepth();
		~R3DStereoDepth();
		void Init(const ParamStereo & param);
		void Compute(
			cv::Mat & depth,
			const cv::Mat & im_aux,
			const cv::Mat & im_main,
			const cv::String & uid,
			float fx, // in ratio form, like (0.5, 0.5) being the center of the image
			float fy);
		float GetFocusDistance(float focus_x, float focus_y); // not available with dynamic_rect=True
		void GetDepth(const cv::Mat & disp_u8, cv::Mat & depth_f32);
		void VisualizeDepth(const cv::Mat & depth_f32, cv::Mat & vis_depth);
		void GetDisparity(const cv::Mat & i_depth_f32, cv::Mat & o_disp);
		void MergeAlphaChannel(
			const cv::Mat & i_rgb,
			const cv::Mat & i_alpha,
			cv::Mat & o_rgba);
		void Get3DPointCloud(
			const cv::Mat & i_rgba, // CV_8UC4
			const cv::Mat & i_depth, // CV_32FC1
			const cv::Mat & i_cmm, // camera matrix in 64FC1
			cv::Mat & o_cloud); // CV_32FC4
		void EstimateNormal(
			const cv::Mat & i_cloud, // CV_32FC4
			cv::Mat & o_normal,// CV_32FC3
			int radius,// searching range
			int sample_rate, // sampleing rate
			float threshold,// searching threshold
			int min_support); // minimum number of neighbors
		void SavePointCloudAsPLY(
			const cv::Mat & cloud, 
			const cv::Mat & normals,
			const char * file,
			float max_depth,
			bool use_ascii=true);
		void Recon3DModel(
			std::vector<Triangle> & mesh, 
			const cv::Mat & cloud, 
			const cv::Mat & normals);

	private:
		ParamStereo m_param; // input parameter list
		cv::String uid; // unique id for logging or dumping
		bool init_success; // indication of initialization status

		cv::Mat depth_u8; // depth map of uint8
		cv::Mat depth_out; // output depth map of uint8

		cv::Mat Q;
		cv::Mat map11, map12, map21, map22;
		cv::Mat imap1, imap2;

		cv::Mat im_L, im_R, im_L_small, im_R_small;
		cv::Mat im_guide; // the guide image

		cv::Size target_size;
		cv::Mat confidence_map;

		cv::Mat aux_warp;
		int zero_flag;

		float cali_w, cali_h;
		float full_w, full_h;
		float crop_w, crop_h;
		float dept_w, dept_h;
		int margin_y;
		int margin_x;
		float real_max_disp;
		float sgbm_max_disp;
		float micro_dist_thr;
		float macro_dist_thr;
		float trigger_max_disp;
		float min_disp;
		double down_scale, scale_aux_x, scale_aux_y;
		cv::Size2f crop_cali_size, crop_delta_size;
		float fx, fy;
		cv::Mat mat_row_align;

		double depth_clip_max;
		double depth_range_max;

		void Preprocess(
			const cv::Mat & inputImg1,
			const cv::Mat & inputImg2);
		int ComputeDepth();
		void Postprocess();

		int ComputeDepthImpl(
			cv::Mat & output,
			const cv::Mat & main_,
			const cv::Mat & aux_);
	};
}

#endif  // R3D_STEREO_DEPTH_H